/* 
Copyright (C) 2005, William Donnelly (wdonnelly@uwaterloo.ca)
and Stefanus Du Toit (sjdutoit@cgl.uwaterloo.ca)

This software is provided 'as-is', without any express or implied
warranty.  In no event will the authors be held liable for any damages
arising from the use of this software.

Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely, subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software
in a product, an acknowledgment in the product documentation would be
appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#include "distance.h"
#include <cmath>
#include <limits>
#include <fstream>
using namespace std;
// A type to hold the distance map while it's being constructed

using namespace SH;

DistanceMap distance_map_algo(const ShImage& heightmap, int depth)
{

	// Initialize the distance map to zero below the surface,
	// and +infinity above
	DistanceMap dmap(heightmap.width(), heightmap.height(), depth);
	for (int y = 0; y < heightmap.height(); y++) {
		for (int x = 0; x < heightmap.width(); x++) {
			for (int z = 0; z < depth; z++) {
				for (int i = 0; i < 3; i++) {
					if (z < heightmap(x, y, 0) * depth || z <= 1) {

						/*if(  heightmap(x, y, 0) > 0.1 && heightmap(x, y, 0) <0.9 )
						dmap(x, y, z, i) = std::numeric_limits<DistanceMap::Distance>::max();
						else*/
						dmap(x, y, z, i) = 0;
					} else {
						dmap(x, y, z, i) = std::numeric_limits<DistanceMap::Distance>::max();
					}
				}
			}
		}
	}


	// Compute the rest of dmap by sequential sweeps over the data
	// using a 3d variant of Danielsson's algorithm
	std::cerr << "\nComputing Distance Map" << std::endl;

	for (int z = 1; z < depth; z++) {

		std::cerr << ".";

		// combine with everything with dz = -1
		for (int y = 0; y < heightmap.height(); y++) {
			dmap.combine( 0, 0, -1,
				1, 0, 0,
				0, y, z);
		}

		for (int y = 1; y < heightmap.height(); y++) {
			dmap.combine( 0, -1, 0,
				1, 0, 0,
				0, y, z);
			dmap.combine(-1, 0, 0,
				1, 0, 0,
				1, y, z);
			dmap.combine(+1, 0, 0,
				-1, 0, 0,
				heightmap.width() - 2, y, z);
		}

		for (int y = heightmap.height() - 2; y >= 0; y--) {
			dmap.combine( 0, +1, 0,
				1, 0, 0,
				0, y, z);
			dmap.combine(-1, 0, 0,
				1, 0, 0,
				1, y, z);
			dmap.combine(+1, 0, 0,
				-1, 0, 0,
				heightmap.width() - 2, y, z);
		}
	}
	std::cerr << " done first pass" << std::endl;

	for (int z = depth - 2; z >= 0; z--) {
		std::cerr << ".";

		// combine with everything with dz = +1
		for (int y = 0; y < heightmap.height(); y++) {
			dmap.combine( 0, 0, +1,
				1, 0, 0,
				0, y, z);
		}

		for (int y = 1; y < heightmap.height(); y++) {
			dmap.combine( 0, -1, 0,
				1, 0, 0,
				0, y, z);
			dmap.combine(-1, 0, 0,
				1, 0, 0,
				1, y, z);
			dmap.combine(+1, 0, 0,
				-1, 0, 0,
				heightmap.width() - 2, y, z);
		}
		for (int y = heightmap.height() - 2; y >= 0; y--) {
			dmap.combine( 0, +1, 0,
				1, 0, 0,
				0, y, z);
			dmap.combine(-1, 0, 0,
				1, 0, 0,
				1, y, z);
			dmap.combine(+1, 0, 0,
				-1, 0, 0,
				heightmap.width() - 2, y, z);
		}
	}

	std::cerr << " done second pass" << std::endl;

	return dmap;
}



ShImage3D init_distance_map(DistanceMap dmap)
{
	int img_width = dmap.m_width,  img_height = dmap.m_height, img_depth = dmap.m_depth;
	ShImage3D img(img_width, img_height, img_depth, 1);

	for (int z = 0; z < dmap.m_depth; z+=1) {
		for (int y = 0; y < dmap.m_height; y+=1) {
			for (int x = 0; x < dmap.m_width; x+=1) {
				double value = 0;
				for (int i = 0; i < 3; i++) {
					value += dmap(x, y, z, i)*dmap(x, y, z, i);
				}
				value = sqrt(value)/ dmap.m_depth;

				if (value > 1.0) 
					value = 1.0;

				img(x, y, z, 0) = value;
			}
		}
	}
	return img;
}

float* init_distance_map_float(DistanceMap dmap)
{
	int img_width = dmap.m_width,  img_height = dmap.m_height, img_depth = dmap.m_depth;
	float* img = new float[img_depth*	img_height*img_width];


	for (int z = 0; z < dmap.m_depth; z+=1) {
		for (int y = 0; y < dmap.m_height; y+=1) {
			for (int x = 0; x < dmap.m_width; x+=1) {
				double value = 0;
				for (int i = 0; i < 3; i++) {
					value += dmap(x, y, z, i)*dmap(x, y, z, i);
				}
				value = sqrt(value)/ dmap.m_depth;

				if (value > 1.0) value = 1.0;

				//img(x, y, z, 0) = value;
				img[z*dmap.m_height*dmap.m_width + y*dmap.m_width + x ] = value;
			}
		}
	}
	return img;
}

ShImage3D distanceMap_minimum(const ShImage3D& distMap)
{

	int resolution = 2;
	int img_width = distMap.width()/resolution,  img_height = distMap.height()/resolution, img_depth = distMap.depth()/resolution;
	ShImage3D img(img_width, img_height, img_depth, 1);


	for (int x = 0; x < distMap.width(); x+=resolution) 
	{
		for (int y = 0; y < distMap.height(); y+=resolution) 
		{
			for (int z = 0; z < distMap.depth(); z+=resolution) 
			{
				float minValue = distMap(x, y, z, 0);

				for(int ii=0; ii<resolution; ii++)
					for(int jj=0; jj<resolution; jj++)
						for(int kk=0; kk<resolution; kk++)
						{
							if(minValue > distMap(x+ii, y+jj, z+kk, 0) )
								minValue= distMap(x+ii, y+jj, z+kk, 0);

						}

						img(x/resolution, y/resolution, z/resolution, 0) = minValue;
			}
		}
	}

	return img;
}


ShImage CalcMeanCurvImage(float *zbuf, int width, int height)
{
	float fx, fy;
	float	fxx, fyy, fxy;
	float tmp;

	int i, j, yoffset, offset;
	yoffset = 0;

	ShImage img(width, height, 1);


	for (int y = 0; y < height; y++) 
	{
		for (int x = 0; x < width; x++) 
		{
			img(x, y,  0) = 0.0;
		}
	}

	for (j = 1; j < height - 1; j++)
	{
		yoffset += width;
		for (i = 1; i < width - 1; i++)
		{
			offset = yoffset + i;
			fx = (zbuf[offset+1] - zbuf[offset-1]) / 2;
			fy = (zbuf[offset+width] - zbuf[offset-width]) /2;
			fxx = (zbuf[offset-1] - 2*zbuf[offset] + zbuf[offset+1]) / 4;
			fyy = (zbuf[offset-width] - 2 * zbuf[offset] + zbuf[offset+width]) / 4;
			fxy = (-zbuf[offset-1-width] + zbuf[offset+1-width])/4 +
				(zbuf[offset-1+width] - zbuf[offset+1+width])/4;
			tmp =	(fxx + fyy + fxx*fy*fy + fyy*fx*fx - 2*fx*fy*fxy) /
				(2 * pow( (1+fx*fx+ fy*fy), (float)1.5 ));

			img(i, j,  0) = abs(tmp);
		}		
	}

	return img;
}

ShImage init_curvature_map_image(const ShImage& heightmap, int resolution)
{
	int width = heightmap.width(), height = heightmap.height();
	int buff_width = width/resolution, buff_height = height/resolution;
	int bx, by;
	float *zbuffer = new float[buff_width*buff_height];

	for (int y = 0; y < height; y+=resolution) 
	{
		by = y/resolution;

		for (int x = 0; x < width; x+=resolution) 
		{
			bx = x/resolution;
			zbuffer[by*buff_width + bx ] = heightmap(x, y, 0) * 10; 
		}
	}

	return CalcMeanCurvImage( zbuffer, buff_width, buff_height );
}

ShImage3D CalcMeanCurv(float *zbuf, int width, int height, int depth)
{
	float fx, fy;
	float	fxx, fyy, fxy;
	float tmp, var_save;

	int i, j, yoffset, offset;
	yoffset = 0;

	ShImage3D img(width, height, depth, 1);

	for (int z = 0; z < depth; z++) 
	{
		for (int y = 0; y < height; y++) 
		{
			for (int x = 0; x < width; x++) 
			{
				img(x, y, z, 0) = 0.0;
			}
		}
	}

	for (j = 1; j < height - 1; j++)
	{
		yoffset += width;
		for (i = 1; i < width - 1; i++)
		{
			offset = yoffset + i;
			fx = (zbuf[offset+1] - zbuf[offset-1]) / 2;
			fy = (zbuf[offset+width] - zbuf[offset-width]) /2;
			fxx = (zbuf[offset-1] - 2*zbuf[offset] + zbuf[offset+1]) / 4;
			fyy = (zbuf[offset-width] - 2 * zbuf[offset] + zbuf[offset+width]) / 4;
			fxy = (-zbuf[offset-1-width] + zbuf[offset+1-width])/4 +
				(zbuf[offset-1+width] - zbuf[offset+1+width])/4;
			tmp =	(fxx + fyy + fxx*fy*fy + fyy*fx*fx - 2*fx*fy*fxy) /
				(2 * pow( (1+fx*fx+ fy*fy), (float)1.5 ));

			int zz = zbuf[offset]* (depth-1) ;
			var_save = abs(tmp)*10;

			for(int kk=0; kk<=zz; kk++)
				img(i, j, kk, 0) = var_save;
		}		
	}
	return img;
}

ShImage3D CalcGaussianCurv(float *zbuf, int width, int height, int depth)
{
	float fx, fy;
	float	fxx, fyy, fxy;
	float tmp, var_save;

	int i, j, yoffset, offset;
	yoffset = 0;

	ShImage3D img(width, height, depth, 1);

	for (int z = 0; z < depth; z++) 
	{
		for (int y = 0; y < height; y++) 
		{
			for (int x = 0; x < width; x++) 
			{
				img(x, y, z, 0) = 0.0;
			}
		}
	}

	for (j = 1; j < height - 1; j++)
	{
		yoffset += width;
		for (i = 1; i < width - 1; i++)
		{
			offset = yoffset + i;
			fx = (zbuf[offset+1] - zbuf[offset-1]) / 2;
			fy = (zbuf[offset+width] - zbuf[offset-width]) /2;
			fxx = (zbuf[offset-1] - 2*zbuf[offset] + zbuf[offset+1]) / 4;
			fyy = (zbuf[offset-width] - 2 * zbuf[offset] + zbuf[offset+width]) / 4;
			fxy = (-zbuf[offset-1-width] + zbuf[offset+1-width])/4 +
				(zbuf[offset-1+width] - zbuf[offset+1+width])/4;
			tmp =	(fxx*fyy-fxy*fxy)/pow((1+fx*fx+fy*fy), 2);
			/*
			(fxx + fyy + fxx*fy*fy + fyy*fx*fx - 2*fx*fy*fxy) /
			(2 * pow( (1+fx*fx+ fy*fy), (float)1.5 ));
			*/		
			int zz = zbuf[offset]* (depth-1) ;
			var_save = abs(tmp)*10;
			img(i, j, zz, 0) = var_save;
		}		
	}
	return img;
}

ShImage3D init_curvature_map(const ShImage& heightmap, int depth)
{
	int width = heightmap.width(), height = heightmap.height();
	int buff_width = width, buff_height = height, buff_depth = depth;
	int bx, by;
	float *zbuffer = new float[buff_width*buff_height];
	float debug;

	for (int y = 0; y < height; y+=1) 
	{
		by = y;
		for (int x = 0; x < width; x+=1) 
		{
			bx = x;
			zbuffer[by*buff_width + bx ] = heightmap(x, y, 0); 
			debug = heightmap(x, y, 0);
		}
	}
	return CalcMeanCurv( zbuffer, buff_width, buff_height, buff_depth );
	//return CalcGaussianCurv( zbuffer, buff_width, buff_height, buff_depth );
}



void curvatureMap_resolution(const ShImage3D& curvMap, float* img,  int w, int h, int d, int resolution)
{	
	int img_width = w/resolution,  img_height = h/resolution, img_depth = d/resolution;
	float maxValue;

	for (int x = 0; x < w; x+=resolution) 
		for (int y = 0; y < h; y+=resolution) 
			for (int z = 0; z < d; z+=resolution) 
			{
				maxValue = -2;

				for(int ii=0; ii<resolution; ii++)
					for(int jj=0; jj<resolution; jj++)
						for(int kk=0; kk<resolution; kk++)
						{
							if( curvMap(x+ii, y+jj, z+kk, 0) > maxValue )
								maxValue = curvMap(x+ii, y+jj, z+kk, 0);
						}

						img[ (z/resolution)*img_width*img_height + (y/resolution)*img_width + x/resolution ] = maxValue;
			}

}



ShImage HeightMap_resolution(const ShImage& curvMap, int resolution)
{

	int img_width = curvMap.width()/resolution,  img_height = curvMap.height()/resolution;
	ShImage img(img_width, img_height, 1);
	float avgValue;
	int count;

	for (int x = 0; x < curvMap.width(); x+=resolution) 
	{
		for (int y = 0; y < curvMap.height(); y+=resolution) 
		{
			avgValue = 0;
			count = 0;

			for(int ii=0; ii<resolution; ii++)
				for(int jj=0; jj<resolution; jj++)
				{
					avgValue+= curvMap(x+ii, y+jj, 0);
					count++;
				}

				img(x/resolution, y/resolution,  0) = avgValue/count;

		}
	}

	return img;
}
